#include <iostream>
#include <vector>
#include <boost/shared_ptr.hpp>
#include "ros/ros.h"
#include "string"
#include "stanley_controller.hpp"
#include "getPath.hpp"
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>

using namespace std;

int main(int argc, char** argv)
{
	ros::init(argc, argv, "stanley_track_node");
	ros::NodeHandle nh("~");

	vector<State> path;
	getPath(path);

	Stanley st(nh, path);
	ros::Rate loop_rate(10);
	while (ros::ok())
	{
		if (!st.isArrived())
			st.loop();
		else
			ROS_INFO("The vehicle has reached the destination!!!");
		loop_rate.sleep();
	}

	return 0;
}